#include <cv.h>
#include <highgui.h>
#include <stdio.h>
#include <stdlib.h>
#include <string>

#include < opencv2\opencv.hpp>  
#include <vector>

//#define Calib

int n_boards = 0;		//Number of snapshots of the chessboard
int frame_step;			//Frames to be skipped
int board_w;			//Enclosed corners horizontally on the chessboard
int board_h;			//Enclosed corners vertically on the chessboard

std::string getTimeinString()
{
    time_t timeObj;
    time(&timeObj);
    tm *pTime = gmtime(&timeObj);
    char buffer[100];
	sprintf(buffer, "%02d%02d%02d_%02d%02d%02d",    pTime->tm_year+1900,pTime->tm_mon+1,pTime->tm_mday,
		pTime->tm_hour+7, pTime->tm_min, pTime->tm_sec);
    return buffer;
}

int main() 
{
	//std::string fileInput = "http://root:pass@172.16.123.1/axis-cgi/mjpg/video.cgi?resolution=640x480&req_fps=30&.mjpg";
	std::string fileInput = "D:/!Projects/!ExampleProjects/Data/Calibration/123/Videos/Source2_20140716_142640.avi";

	std::string fileIntrinsics = "Test123_2_69_Intrinsics.xml";
	std::string fileDistortion = "Test123_2_69_Distortion.xml";
	
	CvMat* intrinsic_matrix  = cvCreateMat(3,3,CV_32FC1);
	CvMat* distortion_coeffs = cvCreateMat(5,1,CV_32FC1);
	
#ifdef Calib

	printf("Enter the numbers of spanspots = ");
	scanf("%d",&n_boards);
	
	printf("\rEnter the numbers of frames to skip = ");
	scanf("%d",&frame_step);

	board_w  = 11;
	board_h  = 7;
  
	int board_total  = board_w * board_h;						//Total enclosed corners on the board
	CvSize board_sz = cvSize( board_w, board_h );

	CvCapture *capture=cvCaptureFromFile("http://root:pass@172.16.100.205/axis-cgi/mjpg/video.cgi?resolution=640x480&req_fps=30&.mjpg");
	//capture = cvCaptureFromAVI("D:/!Projects/!ExampleProjects/Data/Calibration/Source2_20140716_142640.avi");
	if(!capture) 
	{ 
		printf("\nCouldn't open the camera\n"); 
		return -1;
	}

	cvNamedWindow( "Snapshot" );
	cvNamedWindow( "Raw Video");

	//Allocate storage for the parameters according to total number of corners and number of snapshots
	CvMat* image_points      = cvCreateMat(n_boards*board_total,2,CV_32FC1);
	CvMat* object_points     = cvCreateMat(n_boards*board_total,3,CV_32FC1);
	CvMat* point_counts      = cvCreateMat(n_boards,1,CV_32SC1);

	//Note:
	//Intrinsic Matrix - 3x3			   Lens Distorstion Matrix - 4x1
	//	[fx 0 cx]							[k1 k2 p1 p2   k3(optional)]
	//	[0 fy cy]
	//	[0  0  1]


	CvPoint2D32f* corners = new CvPoint2D32f[ board_total ];
	int corner_count;
	int successes = 0;
	int step, frame = 0;

	IplImage *image = cvQueryFrame( capture );
	IplImage *detectedImage = cvCreateImage(cvGetSize(image),8,3);			//subpixel
	IplImage *gray_image = cvCreateImage(cvGetSize(image),8,1);			//subpixel
 
  
	//Loop while successful captures equals total snapshots
	//Successful captures implies when all the enclosed corners are detected from a snapshot
		
	char fileName[100];	
	char *filePath = "Images/";
	char *ext = ".png";
	char *format = "%s%04d%s";
	int iStart = 0;
		
	while(successes < n_boards)					
	{
		if((frame++ % frame_step) == 0)									//Skip frames
		{
			//Find chessboard corners:
			int found = cvFindChessboardCorners(image, board_sz, corners, &corner_count,CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS );			
			cvCvtColor(image, gray_image, CV_BGR2GRAY);										//Get Subpixel accuracy on those corners

			cvCopy(image, detectedImage);

			cvFindCornerSubPix(gray_image, corners, corner_count, cvSize(11,11),cvSize(-1,-1), cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
			cvDrawChessboardCorners(detectedImage, board_sz, corners, corner_count, found);			//Draw it

			std::cout << iStart << std::endl;
			
			// If we got a good board, add it to our data
			if( corner_count == board_total ) 
			{	
				//Save Image
				sprintf(fileName, "%s%04d%s", filePath, iStart++, ext);
				cvSaveImage(fileName, image);

				cvShowImage( "Snapshot", detectedImage );										//show in color if we did collect the image
				//sprintf(fileName, "%Detected%04d%s", filePath, iStart++, ext);
				//cvSaveImage(fileName, detectedImage);

				step = successes*board_total;
				for( int i=step, j=0; j<board_total; ++i,++j ) 
				{
					CV_MAT_ELEM(*image_points, float,i,0) = corners[j].x;
					CV_MAT_ELEM(*image_points, float,i,1) = corners[j].y;
					CV_MAT_ELEM(*object_points,float,i,0) = (float) j/board_w;
					CV_MAT_ELEM(*object_points,float,i,1) = (float) (j%board_w);
					CV_MAT_ELEM(*object_points,float,i,2) = 0.0f;
				}
				CV_MAT_ELEM(*point_counts, int,successes,0) = board_total;    
				successes++;
				printf("\r%d successful Snapshots out of %d collected.",successes,n_boards);
			}
			else
				cvShowImage( "Snapshot", gray_image );										//Show Gray if we didn't collect the image
		} 
		

		//Handle pause/unpause and ESC
		int c = cvWaitKey(15);
		if(c == 'p')
		{  
			c = 0;
			while(c != 'p' && c != 27)
			{
				c = cvWaitKey(250);
			}
		}
		if(c == 27)
			return 0;

		image = cvQueryFrame( capture );								//Get next image
		cvShowImage("Raw Video", image);
	} 

	//End WHILE loop with enough successful captures

	cvDestroyWindow("Snapshot");

	printf("\n\n *** Calbrating the camera now...\n");
	
	//Allocate matrices according to successful number of captures
	CvMat* object_points2  = cvCreateMat(successes*board_total,3,CV_32FC1);
	CvMat* image_points2   = cvCreateMat(successes*board_total,2,CV_32FC1);
	CvMat* point_counts2   = cvCreateMat(successes,1,CV_32SC1);

	//Tranfer the points to matrices
	for(int i = 0; i<successes*board_total; ++i)
	{
      CV_MAT_ELEM( *image_points2, float, i, 0)  =	CV_MAT_ELEM( *image_points, float, i, 0);
      CV_MAT_ELEM( *image_points2, float,i,1)    =	CV_MAT_ELEM( *image_points, float, i, 1);
      CV_MAT_ELEM(*object_points2, float, i, 0)  =  CV_MAT_ELEM( *object_points, float, i, 0) ;
      CV_MAT_ELEM( *object_points2, float, i, 1) =  CV_MAT_ELEM( *object_points, float, i, 1) ;
      CV_MAT_ELEM( *object_points2, float, i, 2) =  CV_MAT_ELEM( *object_points, float, i, 2) ;
	} 
	
	for(int i=0; i<successes; ++i)
	{ 
		CV_MAT_ELEM( *point_counts2, int, i, 0) =  CV_MAT_ELEM( *point_counts, int, i, 0);			//These are all the same number
	}
	cvReleaseMat(&object_points);
	cvReleaseMat(&image_points);
	cvReleaseMat(&point_counts);

	// Initialize the intrinsic matrix with both the two focal lengths in a ratio of 1.0

	CV_MAT_ELEM( *intrinsic_matrix, float, 0, 0 ) = 1.0f;
	CV_MAT_ELEM( *intrinsic_matrix, float, 1, 1 ) = 1.0f;

	//Calibrate the camera
	//_____________________________________________________________________________________
 
	cvCalibrateCamera2(object_points2, image_points2, point_counts2,  cvGetSize( image ), intrinsic_matrix, distortion_coeffs, NULL, NULL,0 );
																																		//CV_CALIB_FIX_ASPECT_RATIO
	//_____________________________________________________________________________________
	
	for(int i=0; i< distortion_coeffs->rows; ++i)  
	{  
		for(int j=0; j< distortion_coeffs->cols; ++j)  
		{  
			printf("%lf ", cvmGet(distortion_coeffs, i,j)); //cvmGet(matrix,i,j));  
		}  
		printf("\n");  
	}  
	printf("\n"); 

	//Save values to file
	printf(" *** Calibration Done!\n\n");
	printf("Storing Intrinsics.xml and Distortions.xml files...\n");
	cvSave(fileIntrinsics.c_str(),intrinsic_matrix);
	cvSave(fileDistortion.c_str(),distortion_coeffs);
	printf("Files saved.\n\n");
#else
	//Open form CameraIP
	//CvCapture *capture=cvCaptureFromFile("http://root:pass@172.16.100.205/axis-cgi/mjpg/video.cgi?resolution=640x480&req_fps=30&.mjpg");
	//Open from file
	//File 1
	//CvCapture *capture=cvCaptureFromFile("D:\\!Projects\\!ExampleProjects\\Data\\Calibration\\205\\Videos\\Source1_20140721_123747.avi");
	//File 2
	CvCapture *capture=cvCaptureFromFile(fileInput.c_str());
	if(!capture) 
	{ 
		printf("\nCouldn't open the camera\n"); 
		return -1;
	}
	IplImage *image = cvQueryFrame( capture );
#endif
	printf("Starting corrected display....");

	//Sample: load the matrices from the file
	CvMat *intrinsic = (CvMat*)cvLoad(fileIntrinsics.c_str());
	CvMat *distortion = (CvMat*)cvLoad(fileDistortion.c_str());

//Initialize cvMat
	/*CvMat *intrinsic = cvCreateMat( 3, 3, CV_32FC1 );
	cvZero( intrinsic );
	cvmSet( intrinsic, 0, 0, 536.5182026796776 );
	cvmSet( intrinsic, 0, 1, 0. );
	cvmSet( intrinsic, 0, 2, 326.9434382591148 );

	cvmSet( intrinsic, 1, 0, 0. );
	cvmSet( intrinsic, 1, 1, 503.8081613863488 );
	cvmSet( intrinsic, 1, 2, 273.6197178458114 );

	cvmSet( intrinsic, 2, 0, 0. );
	cvmSet( intrinsic, 2, 1, 0. );
	cvmSet( intrinsic, 2, 2, 1. );

	CvMat *distortion = cvCreateMat( 5, 1, CV_32FC1 );
	cvZero( distortion );
	cvmSet( distortion, 0, 0, -0.446990 );
	cvmSet( distortion, 1, 0, 0.250725 );
	cvmSet( distortion, 2, 0, -0.000127 );
	cvmSet( distortion, 3, 0, 0.000469);
	cvmSet( distortion, 4, 0, -0.089500 );*/

// Build the undistort map used for all subsequent frames.

	IplImage* mapx = cvCreateImage( cvGetSize(image), IPL_DEPTH_32F, 1 );
	IplImage* mapy = cvCreateImage( cvGetSize(image), IPL_DEPTH_32F, 1 );
	cvInitUndistortMap(intrinsic,distortion,mapx,mapy);

	// Run the camera to the screen, showing the raw and the undistorted image.

	cvNamedWindow( "Undistort" );
	int nFame = 0;
	while(image) 
	{
		std::cout << "idFrame = " << nFame++ << std::endl;

		IplImage *t = cvCloneImage(image);
		cvShowImage( "Raw Video", image );			// Show raw image
		cvRemap( t, image, mapx, mapy );			// Undistort image
		cvReleaseImage(&t);
		cvShowImage("Undistort", image);			// Show corrected image

		//Handle pause/unpause and ESC
		int c = cvWaitKey(15);
		if(c == 'p')
		{ 
			c = 0;
			//
			IplImage *frame = cvLoadImage("Image_rect1.jpg");
			IplImage *p = cvCloneImage(frame);
			
			cvRemap( p, frame, mapx, mapy );			// Undistort image
			cvReleaseImage(&p);
			cvShowImage("Image_rect31 Undistort", frame);

			std::string fileOut = "out_" + getTimeinString() + ".jpg";
			cvSaveImage(fileOut.c_str(), frame);

			while(c != 'p' && c != 27)
			{
				c = cvWaitKey(250);
			}
		}
		if(c == 27)
			break;

		image = cvQueryFrame( capture );

		if (!image)
		{
			capture = cvCaptureFromFile(fileInput.c_str());
			if(!capture) 
			{ 
				printf("\nCouldn't open the camera\n"); 
				return -1;
			}
			nFame = 0;
			image = cvQueryFrame(capture );
		}
	} 
	
	return 0;
}



